from numpy import *
from numpy.linalg import *
from math import *
class sensor_goal(object):
	"""This sensor gives you the relative distance and angle to the agents 'goal' position."""
	def __init__(self, agent_num, world_model,goal_pos,max_range=120):
		self.agent_num=agent_num
		self.wm=world_model
		self.goal_pos=array(goal_pos)
		self.max_range=max_range
	def sense(self):
	    pos=self.goal_pos
	    cur_pos=array(self.wm.objects[self.agent_num].pos)
	    cur_heading=self.wm.objects[self.agent_num].heading
	    diff_vec=pos-cur_pos
	    target_magnitude=min(norm(diff_vec),self.max_range)
	    target_heading=((atan2(diff_vec[1],diff_vec[0])-cur_heading) % (2*pi))-pi
	    return target_magnitude,target_heading
